/**
 * @file posctl_hover.cpp
 * @brief Offboard control example node in POSITION control mode
 */

#include "PX4CtrlFSM.h"

px4ctrlfsm_msg::DebugPid msgDebugPid;

/**************************** Function Declaration and Definition *******************************/
PX4CtrlFSM::PX4CtrlFSM(ros::NodeHandle& nh) {
    pubPID = nh.advertise<px4ctrlfsm_msg::DebugPid>("/controller/pid", 1);
    pubPx4Attitude = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 1);
    pubDroneTargetEulerThrust = nh.advertise<geometry_msgs::PoseStamped>("/controller/drone_target_euler_thrust", 1);
    pubTargetPose = nh.advertise<offboard::PosVelAcc>("/controller/drone_target_pose", 1);
    pubRCfeed = nh.advertise<std_msgs::Float64MultiArray>("/controller/drone_rc_feed", 1);
}

/**
 * @brief 析构函数
 */
PX4CtrlFSM::~PX4CtrlFSM(){
}

/**
 * @brief pose2pva_hover: 将pose参数转化为pva
 */
void PX4CtrlFSM::pose2pva(geometry_msgs::PoseStamped pose, offboard::PosVelAcc &pva)
{
    pva.px = pose.pose.position.x;
    pva.py = pose.pose.position.y;
    pva.pz = pose.pose.position.z;
    pva.vx = 0.0;
    pva.vy = 0.0;
    pva.vz = 0.0;
    pva.ax = 0.0;
    pva.ay = 0.0;
    pva.az = 0.0;
    pva.yaw = 0.0;
    targetPoseLastTimeStamp = ros::Time::now().toSec();
}


// callback function
void PX4CtrlFSM::uav_state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    uav_cur_state = *msg;
}

void PX4CtrlFSM::uav_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    uav_cur_pose = *msg;
}

void PX4CtrlFSM::uav_poseOdom_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    uav_cur_poseOdom = *msg;
}

// /**
//  * @brief 读取euler
//  */
// void PX4CtrlFSM::euler_in_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
// {
//     uav_cur_euler = *msg;
// }

/**
 * @brief 读取遥控器信号,并判断当前rc对应的mode
 */
void PX4CtrlFSM::rc_in_cb(const mavros_msgs::RCIn::ConstPtr& msg)
{
    // ============ MODE: 获取并处理mode切换 对应channel6 ============ //
    rc_in_feedback = msg->channels[mode_channel]; // channel 6
    if (rc_in_feedback >= 1000 && rc_in_feedback <= 1100) {
    //    ROS_INFO("Channel 6 switched on: %d --- landing",rc_in_feedback);
        rc_data.landing = true;
        rc_data.hover = false;
        rc_data.takeoff = false;
        rc_data.docking = false;
    } else if(rc_in_feedback >= 1500 && rc_in_feedback <= 1600) {
    //    ROS_INFO("Channel 6 switched on: %d --- hover",rc_in_feedback);
        rc_data.hover = true;
        rc_data.landing = false;
        rc_data.takeoff = false;
        // rc_data.docking = false;
    } else if (rc_in_feedback >= 1900 && rc_in_feedback <= 2000) {
    //    ROS_INFO("Channel 6 switched on: %d --- takeoff",rc_in_feedback);
        rc_data.takeoff = true;
        rc_data.landing = false;
        rc_data.hover = false;
        rc_data.docking = false;
    } else {
        ROS_WARN("==================Channel 6 in unknown position=============: %d", rc_in_feedback);
        // 在此处执行相应的操作，这里包括处理不在预期范围内的值的情况
        rc_data.landing = false;
        rc_data.hover = false;
        rc_data.takeoff = false;
        rc_data.docking = false;
    }

    // ============ MODE: 获取并处理 mdock 切换 对应 channel dock_channel ============ //
    rc_in_feedback_dock = msg->channels[dock_channel]; // channel 7

    // if(rc_data.hover){
    //     // ROS_WARN("Channel dock switched on: %d", rc_in_feedback_dock);
    //     ROS_WARN_THROTTLE(1, "Channel dock switched on: %d", rc_in_feedback_dock);

    // }
    // ROS_WARN("Channel dock switched on: %d", rc_in_feedback_dock);

    if (rc_in_feedback_dock >= 1900 && rc_in_feedback_dock <= 2000) {
        if (!was_docking_activated) {
            // 第一次进入 docking 范围
            ROS_INFO("Channel dock activated: true, docking state toggled");
            rc_data.docking = !rc_data.docking; // 切换状态
            was_docking_activated = true; // 标记为已激活
        }
    } else if (rc_in_feedback_dock >= 1000 && rc_in_feedback_dock <= 1100) {
        was_docking_activated = false; // 标记为未激活
    } else {
        ROS_WARN("Channel 7 in unknown position: %d, docking state: %d", rc_in_feedback_dock, rc_data.docking);
        // 处理不在预期范围内的值的情况
        was_docking_activated = false; // 标记为未激活
        rc_data.docking = false;       // 可根据需要保留或删除
    }

    // ============  RC: 获取遥控器输入信号的俯仰通道 ============ //
    rc_in_pitch_feedback = msg->channels[pitch_channel];
    // 在中值附近取0
    if (abs(rc_in_pitch_feedback - rcSignalRange_x.mid)<= 50){
        rc_in_pitch_feedback = rcSignalRange_x.mid;
    }

    // ============  RC: 获取遥控器输入信号的横滚值 ============ //
    rc_in_roll_feedback = msg->channels[roll_channel];
    // 在中值附近取0
    rc_in_roll_feedback = msg->channels[roll_channel];
    if (abs(rc_in_roll_feedback - rcSignalRange_y.mid)<= 50){
        rc_in_roll_feedback = rcSignalRange_y.mid;
    }

    // ============  RC: 获取遥控器输入信号的油门通道 ============ //
    
    rc_in_thro_feedback_last = rc_in_thro_feedback;
    rc_in_thro_feedback = msg->channels[throttle_channel];
    // ROS_WARN("rc_in_pitch_feedback: %d, rc_in_roll_feedback: %d, rc_in_thro_feedback: %d,rc_in_thro_feedback_last: %d", 
    // rc_in_pitch_feedback, rc_in_roll_feedback, rc_in_thro_feedback, rc_in_thro_feedback_last);
    
    // tubian
    if (abs(rc_in_thro_feedback - rc_in_thro_feedback_last)>= 200 && rc_in_thro_feedback_last !=0){
        rc_in_thro_feedback = rc_in_thro_feedback_last;
    }
    // 在中值附近取0
    if (rc_in_thro_feedback - rcSignalRange_z.mid <= 70 && rc_in_thro_feedback - rcSignalRange_z.mid >= -80){
        rc_in_thro_feedback = rcSignalRange_z.mid;
    }
    if (abs(rc_in_thro_feedback - rcSignalRange_z.max)<= 15 && flight_mode == LANDING){
        landing_process1_flag = true;
    }
    
    // ROS_WARN("rc_in_thro_feedback: %d,rc_in_thro_feedback_last: %d", 
    //         rc_in_thro_feedback, rc_in_thro_feedback_last);
    // 根据飞行模式选择参数
    switch (flight_mode) {
        case HOVER:
            current_params = hover_params;
            break;
        case DOCKING:
            current_params = zero_params;  // now we don't use rc as input, just keep hovering
            break;
        case LANDING:
            current_params = landing_process1_flag ? landing2_params : landing1_params;
            break;
        case IDLE:
            current_params = zero_params;
            break;
        case TAKEOFF:
            current_params = zero_params;
            break;
        // default:
        //     current_params = &zero_params;
        //     break;
    }

    // ============  在HOVER/LANDING模式下可以进行rc调整 ============ //
    // LANDING模式只能调整xy, 且landing_process_2 比例范围比 landing_process_1 更小
    rc_feed.x = rc_feed.x - current_params.rc_rate.x * 0.5 * (current_params.rc_range.max_xy - current_params.rc_range.min_xy)
                * (double)(rc_in_pitch_feedback - rcSignalRange_x.mid) / (0.5*(rcSignalRange_x.max - rcSignalRange_x.min));
    rc_feed.x = std::min(std::max(rc_feed.x, current_params.rc_range.min_xy), current_params.rc_range.max_xy);

    rc_feed.y = rc_feed.y - current_params.rc_rate.y * 0.5 * (current_params.rc_range.max_xy - current_params.rc_range.min_xy)
                * (double)(rc_in_roll_feedback - rcSignalRange_y.mid) / (0.5*(rcSignalRange_y.max - rcSignalRange_y.min));
    rc_feed.y = std::min(std::max(rc_feed.y, current_params.rc_range.min_xy), current_params.rc_range.max_xy);

    // 映射 RC-z
    // rc_feed.z = -current_params.rc_rate.z * 0.5 * (current_params.rc_range.max_z - current_params.rc_range.min_z)
    //             * (double)(rc_in_thro_feedback - rcSignalRange_z.mid) / (0.5*(rcSignalRange_z.max - rcSignalRange_z.min));
    // rc_feed.z = std::min(std::max(rc_feed.z, current_params.rc_range.min_z), current_params.rc_range.max_z);

    // 积分 RC-z
    rc_feed.z = rc_feed.z - current_params.rc_rate.z * 0.5 * (current_params.rc_range.max_z - current_params.rc_range.min_z)
                * (double)(rc_in_thro_feedback - rcSignalRange_z.mid) / (0.5*(rcSignalRange_z.max - rcSignalRange_z.min));
    rc_feed.z = std::min(std::max(rc_feed.z, current_params.rc_range.min_z), current_params.rc_range.max_z);

    // ROS_WARN("------------- Joystick Feedback -------------");
    // ROS_WARN("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", current_params.rc_rate.x, current_params.rc_rate.y, current_params.rc_rate.z);
    // ROS_WARN("rc_rangMIN: %.3f, rc_rangMAX: %.3f", current_params.rc_range.min_xy, current_params.rc_range.max_xy);

    rc_output_msg.data = {rc_feed.x,rc_feed.y,rc_feed.z,current_params.rc_rate.x,current_params.rc_rate.y,current_params.rc_rate.z,
    current_params.rc_range.min_xy, current_params.rc_range.max_xy, current_params.rc_range.min_z, current_params.rc_range.max_z,
    (float)rc_in_pitch_feedback, (float)rc_in_roll_feedback, (float)rc_in_thro_feedback};
    // 发布消息
    pubRCfeed.publish(rc_output_msg);
}

/**
 * @brief 从读取cfg/uav.yaml文件,获取PID和thrust参数
 */
void PX4CtrlFSM::loadParameters(ros::NodeHandle& nh) {
    const std::string& node_name = ros::this_node::getName();
    nh.getParam(node_name + "/" + "BaseThrust", rosParamBaseThrust);
    nh.getParam(node_name + "/" + "ThrustLimit", rosParamThrustLimit);
    nh.getParam(node_name + "/" + "posKP", rosParamPosKP);
    nh.getParam(node_name + "/" + "posKI", rosParamPosKI);
    nh.getParam(node_name + "/" + "posKD", rosParamPosKD);
    nh.getParam(node_name + "/" + "velKP", rosParamVelKP);
    nh.getParam(node_name + "/" + "velKI", rosParamVelKI);
    nh.getParam(node_name + "/" + "velKD", rosParamVelKD);

    nh.getParam(node_name + "/" + "hover_params/rc_rate/x", hover_params.rc_rate.x);
    nh.getParam(node_name + "/" + "hover_params/rc_rate/y", hover_params.rc_rate.y);
    nh.getParam(node_name + "/" + "hover_params/rc_rate/z", hover_params.rc_rate.z);

    nh.getParam(node_name + "/" + "hover_params/rc_range/min_xy", hover_params.rc_range.min_xy);
    nh.getParam(node_name + "/" + "hover_params/rc_range/max_xy", hover_params.rc_range.max_xy);
    nh.getParam(node_name + "/" + "hover_params/rc_range/min_z", hover_params.rc_range.min_z);
    nh.getParam(node_name + "/" + "hover_params/rc_range/max_z", hover_params.rc_range.max_z);

    nh.getParam(node_name + "/" + "landing1_params/rc_rate/x", landing1_params.rc_rate.x);
    nh.getParam(node_name + "/" + "landing1_params/rc_rate/y", landing1_params.rc_rate.y);
    nh.getParam(node_name + "/" + "landing1_params/rc_rate/z", landing1_params.rc_rate.z);

    nh.getParam(node_name + "/" + "landing1_params/rc_range/min_xy", landing1_params.rc_range.min_xy);
    nh.getParam(node_name + "/" + "landing1_params/rc_range/max_xy", landing1_params.rc_range.max_xy);
    nh.getParam(node_name + "/" + "landing1_params/rc_range/min_z", landing1_params.rc_range.min_z);
    nh.getParam(node_name + "/" + "landing1_params/rc_range/max_z", landing1_params.rc_range.max_z);

    nh.getParam(node_name + "/" + "landing2_params/rc_rate/x", landing2_params.rc_rate.x);
    nh.getParam(node_name + "/" + "landing2_params/rc_rate/y", landing2_params.rc_rate.y);
    nh.getParam(node_name + "/" + "landing2_params/rc_rate/z", landing2_params.rc_rate.z);

    nh.getParam(node_name + "/" + "landing2_params/rc_range/min_xy", landing2_params.rc_range.min_xy);
    nh.getParam(node_name + "/" + "landing2_params/rc_range/max_xy", landing2_params.rc_range.max_xy);
    nh.getParam(node_name + "/" + "landing2_params/rc_range/min_z", landing2_params.rc_range.min_z);
    nh.getParam(node_name + "/" + "landing2_params/rc_range/max_z", landing2_params.rc_range.max_z);

    std::cout << "BaseThrust: " << rosParamBaseThrust << std::endl
                << "rosParamThrustLimit: " << rosParamThrustLimit[0] << "  ~  " << rosParamThrustLimit[1] << std::endl
                << "rcParamXfeedLimit: " << rcParamXfeedLimit[0] << "  ~  " << rcParamXfeedLimit[1] << std::endl
                << "rcParamYfeedLimit: " << rcParamYfeedLimit[0] << "  ~  " << rcParamYfeedLimit[1] << std::endl
                << "rcParamZfeedLimit: " << rcParamZfeedLimit[0] << "  ~  " << rcParamZfeedLimit[1] << std::endl
                << "posKP: " << rosParamPosKP[0] << "    " << rosParamPosKP[1] << "    " << rosParamPosKP[2] << std::endl
                << "velKP: " << rosParamVelKP[0] << "    " << rosParamVelKP[1] << "    " << rosParamVelKP[2] << std::endl
                << "velKI: " << rosParamVelKI[0] << "    " << rosParamVelKI[1] << "    " << rosParamVelKI[2] << std::endl
                << "velKD: " << rosParamVelKD[0] << "    " << rosParamVelKD[1] << "    " << rosParamVelKD[2] << std::endl
                << "hover_rc_rate: " << hover_params.rc_rate.x << "    " << hover_params.rc_rate.y << "    " << hover_params.rc_rate.z << std::endl
                << "hover_rc_range_xy: " << hover_params.rc_range.min_xy << "  ~  " << hover_params.rc_range.max_xy << std::endl
                << "hover_rc_range_z : " << hover_params.rc_range.min_z << "  ~  " << hover_params.rc_range.max_z << std::endl
                << "landing1_rc_rate: " << landing1_params.rc_rate.x << "    " << landing1_params.rc_rate.y << "    " << landing1_params.rc_rate.z << std::endl
                << "landing1_rc_range_xy: " << landing1_params.rc_range.min_xy << "  ~  " << landing1_params.rc_range.max_xy << std::endl
                << "landing1_rc_range_z : " << landing1_params.rc_range.min_z << "  ~  " << landing1_params.rc_range.max_z << std::endl
                << "landing2_rc_rate: " << landing2_params.rc_rate.x << "    " << landing2_params.rc_rate.y << "    " << landing2_params.rc_rate.z << std::endl
                << "landing2_rc_range_xy: " << landing2_params.rc_range.min_xy << "  ~  " << landing2_params.rc_range.max_xy << std::endl
                << "landing2_rc_range_z : " << landing2_params.rc_range.min_z << "  ~  " << landing2_params.rc_range.max_z << std::endl;
    // Check thrust range
    if ((rosParamThrustLimit[1] - rosParamBaseThrust) < 0.05 || (rosParamThrustLimit[0] - rosParamBaseThrust) > -0.05) {
        ROS_WARN_STREAM("The param `ThrustLimit` is unreasonable, EXIT !!!!!!!!");
        return;
    }
}

/**
 * @brief 从rosparam中的参数赋给PID
 */
bool PX4CtrlFSM::px4AttitudeCtlInitRosParam()
{
    /// 使用 rosParam 读取参数
    /// 设置基础油门值
    baseThrust = rosParamBaseThrust;
    /// 设置位置环 PID
    pidPX.setPID(rosParamPosKP[0], rosParamPosKI[0], rosParamPosKD[0]);
    pidPY.setPID(rosParamPosKP[1], rosParamPosKI[1], rosParamPosKD[1]);
    pidPZ.setPID(rosParamPosKP[2], rosParamPosKI[2], rosParamPosKD[2]);
    // 设置位置环积分上限 控制量最大值 误差死区
    // pidPX.set_sat(0.2, 2, 0);
    // pidPY.set_sat(0.2, 2, 0);
    // pidPZ.set_sat(1.5, 3, 0); // TODO: Z轴位置积分上限
    pidPX.set_sat(0.3, 2, 0);
    pidPY.set_sat(0.3, 2, 0);
    pidPZ.set_sat(1.5, 3, 0); // TODO: Z轴位置积分上限
    pidPX.start_intergrate_flag = false;
    pidPY.start_intergrate_flag = false;
    pidPZ.start_intergrate_flag = false;
    /// 设置速度环 PID
    pidVX.setPID(rosParamVelKP[0], rosParamVelKI[0], rosParamVelKD[0]);
    pidVY.setPID(rosParamVelKP[1], rosParamVelKI[1], rosParamVelKD[1]);
    pidVZ.setPID(rosParamVelKP[2], rosParamVelKI[2], rosParamVelKD[2]);
     // 设置速度环积分上限 控制量最大值 误差死区
    pidVX.set_sat(0.2, 2, 0);
    pidVY.set_sat(0.2, 2, 0);
    pidVZ.set_sat(0.3, 3, 0);
    pidVX.start_intergrate_flag = false;
    pidVY.start_intergrate_flag = false;
    pidVZ.start_intergrate_flag = false;
    return true;
}

/**
 * @brief Get the current aircraft attitude angle for initializing the yaw angle
 */
void PX4CtrlFSM::initializeYaw(ros::Rate& rate) {
    count_yaw_init = 0;
    yaw_init = 0.0;
    yaw_last = 0.0;
    yaw_thre = M_PI / (4.0 * LOOPRATE);
    
    while (ros::ok() && count_yaw_init < 30) {
        ros::spinOnce();
        geometry_msgs::Vector3 preCurrEuler = quaternion2Euler_ros(uav_cur_poseOdom.pose.pose.orientation);
        if (abs(preCurrEuler.z) > 0.00001) {
            yaw_init += preCurrEuler.z;
            count_yaw_init++;
        }
        rate.sleep();
    }
    yaw_init = yaw_init / 30.0;
    ROS_INFO("Yaw initialized !! Yaw is %f", yaw_init);
    yaw_last = yaw_init;
}


/**
 * @brief 判断当前有无目标位置传入，若没有，则等待，不需要计算期望姿态
 */
void PX4CtrlFSM::waitForTargetPosition(ros::Rate& rate) {
    while (ros::ok() && uav_cur_state.connected && abs(targetPoseLastTimeStamp - ros::Time::now().toSec()) > 0.1) {
        ros::spinOnce();
        ROS_INFO_THROTTLE(1, "not target position given, targetPoseLastTimeStamp: %f, droneState: %s",
                        targetPoseLastTimeStamp, uav_cur_state.mode.c_str());
        rate.sleep();
    }
}

/**
* 将欧拉角转化为四元数
* @param roll
* @param pitch
* @param yaw
* @return 返回四元数
*/
geometry_msgs::PoseStamped PX4CtrlFSM::euler2quaternion(float roll, float pitch, float yaw) {
    geometry_msgs::PoseStamped temp;
    temp.pose.orientation.w = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + sin(roll / 2) * sin(pitch / 2) * sin(yaw / 2);
    temp.pose.orientation.x = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2);
    temp.pose.orientation.y = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2);
    temp.pose.orientation.z = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2);
    return temp;
}

/**
 * @brief 将四元数转化为欧拉角形式，输入的顺序为x-y-z-w,欧拉角的顺序为Yaw(z)-Pitch(y)-Roll(x)
 */
geometry_msgs::Vector3 PX4CtrlFSM::quaternion2euler(float x, float y, float z, float w)
{
    geometry_msgs::Vector3 temp;
    temp.x = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
    temp.y = asin(2.0 * (w * y - z * x));
    temp.z = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
    return temp;
}

geometry_msgs::Vector3 PX4CtrlFSM::quaternion2Euler_ros(geometry_msgs::Quaternion q)
{
    return quaternion2euler(q.x, q.y, q.z, q.w);
}

/**
 * @brief 最重要的计算函数
 *
 * @param _currTime 当前时间
 * @param CurrPos   当前位置
 * @param CurrVel   当前速度
 * @param TargetPos 目标位置
 * @param TargetVel 目标速度
 * @param TargetAcc 目标加速度
 * @param TargetYaw 目标yaw角度
 * @param state     无人机此时的状态，是否解锁，是否进入OFFBOARD
 */
void PX4CtrlFSM::px4AttitudeCtlPVA(double _currTime,
                       Eigen::Vector3d CurrPos,
                       Eigen::Vector3d CurrVel,
                       Eigen::Vector3d TargetPos,
                       Eigen::Vector3d TargetVel,
                       Eigen::Vector3d TargetAcc,
                       double TargetYaw,
                       mavros_msgs::State &state)
{
    /// 如果处于Offboard状态再用积分环节
    if (state.mode == "OFFBOARD")
    {
        /// 位于较好的初始位置，再启用积分环节
        // ROS_WARN("(CurrPos - TargetPos).norm(): %.3f",(CurrPos - TargetPos).norm());
        if ((CurrPos - TargetPos).norm() < 1)
        {
            // ROS_WARN_ONCE("[Controller] Start intergrate in PID");
            ROS_WARN_ONCE("[Controller] Start intergrate in PID");
            pidPX.start_intergrate_flag = true;
            pidPY.start_intergrate_flag = true;
            pidPZ.start_intergrate_flag = true;
            pidVX.start_intergrate_flag = true;
            pidVY.start_intergrate_flag = true;
            pidVZ.start_intergrate_flag = true;
        }
        // else{  // TODO
        //     ROS_WARN_ONCE("[Controller] Stop intergrate in PID");
        //     pidPX.start_intergrate_flag = false;
        //     pidPY.start_intergrate_flag = false;
        //     pidPZ.start_intergrate_flag = false;
        //     pidVX.start_intergrate_flag = false;
        //     pidVY.start_intergrate_flag = false;
        //     pidVZ.start_intergrate_flag = false;
        // }
    }

    // 位置环误差计算
    Eigen::Vector3d posError, velError;
    posError = TargetPos - CurrPos;
    velError = TargetVel - CurrVel;
    pidPX.add_error(posError.x(), _currTime);
    pidPY.add_error(posError.y(), _currTime);
    pidPZ.add_error(posError.z(), _currTime);
    pidVX.add_error(velError.x(), _currTime);
    pidVY.add_error(velError.y(), _currTime);
    pidVZ.add_error(velError.z(), _currTime);

    pidPX.pid_output();
    pidPY.pid_output();
    pidPZ.pid_output();
    pidVX.pid_output();
    pidVY.pid_output();
    pidVZ.pid_output();

    Eigen::Vector3d accExcept;
    Eigen::Vector3d accExcept_g;
    accExcept << pidPX.Output + pidVX.Output + TargetAcc[0],
        pidPY.Output + pidVY.Output + TargetAcc[1],
        pidPZ.Output + pidVZ.Output + TargetAcc[2];

    // TODO:限制Z方向加速度
    accExcept[2] = min(8.0, max(accExcept[2], -8.0));
    // check the thres
    if(accExcept[2]>=8.0 || accExcept[2]<=-8.0){
        ROS_WARN("accExcept[2] :  %f",accExcept[2]);
    }
    if(pidPX.Output>=2.0){
        ROS_WARN("pidPX.pid_output(); :  %.3f",pidPX.Output);
    }
    if(pidPY.Output>=2.0){
        ROS_WARN("pidPY.pid_output(); :  %.3f",pidPY.Output);
    }
    if(pidPZ.Output>=3.0){
        ROS_WARN("pidPZ.pid_output(); :  %.3f",pidPZ.Output);
    }

    // extend a landing mode
    if(flight_mode == LANDING){
        // TODO:限制Z方向加速度
        accExcept[2] = min(5.0, max(accExcept[2], -5.0));
        // check the thres
        if(accExcept[2]>=5.0 || accExcept[2]<=-5.0){
            ROS_WARN("LANDING:accExcept[2] :  %f",accExcept[2]);
        }
    }


    // ROS_WARN("accExcept[2] :  %f",accExcept[2]);
    // ROS_WARN("pidPX.pid_output(); :  %.3f",pidPX.Output);
    // ROS_WARN("pidPY.pid_output(); :  %.3f",pidPY.Output);
    // ROS_WARN("pidPZ.pid_output(); :  %.3f",pidPZ.Output);
    // ROS_WARN("pidVX.pid_output(); :  %.3f",pidVX.Output);
    // ROS_WARN("pidVY.pid_output(); :  %.3f",pidVY.Output);
    // ROS_WARN("pidVZ.pid_output(); :  %.3f",pidVZ.Output);

    accExcept_g = accExcept + Eigen::Vector3d(0, 0, 1) * GRAVITATIONAL_ACC;
    // ROS_WARN("pidPX.Output :  %.3f",pidPX.Output);
    // ROS_WARN("pidPZ.Output :  %.3f",pidPZ.Output);
    // ROS_WARN("accExcept :  %d",accExcept);

    // TODO:Limit control angle to 15 degree
    double theta_ = M_PI * 15.0 / 180.0;
    double cos_ = cos(theta_);
    if (Eigen::Vector3d(0, 0, 1).dot(accExcept_g.normalized()) < cos_)
    {
        double nf_ = accExcept.norm();
        double A_ = cos_ * cos_ * nf_ * nf_ - accExcept(2) * accExcept(2);
        double B_ = 2 * (cos_ * cos_ - 1) * accExcept(2) * GRAVITATIONAL_ACC;
        double C_ = (cos_ * cos_ - 1) * GRAVITATIONAL_ACC * GRAVITATIONAL_ACC;
        double s = (-B_ + sqrt(B_ * B_ - 4 * A_ * C_)) / (2 * A_);
        accExcept_g.noalias() = s * accExcept + GRAVITATIONAL_ACC * Eigen::Vector3d(0, 0, 1);
        ROS_WARN("The control reaches the limitation");
    }
    // Limit control angle to 10 degree

    // Add control for yaw.
    Eigen::Vector3d b1c, b2c, b3c;
    Eigen::Vector3d b2d(-sin(TargetYaw), cos(TargetYaw), 0);
    if (accExcept_g.norm() > 1e-6)
    {
        b3c.noalias() = accExcept_g.normalized();
    }
    else
    {
        b3c.noalias() = Eigen::Vector3d(0, 0, 1);
    }
    b1c.noalias() = b2d.cross(b3c).normalized();
    b2c.noalias() = b3c.cross(b1c).normalized();
    Eigen::Matrix3d R_;
    Eigen::Quaterniond qua_;
    R_ << b1c, b2c, b3c;
    qua_ = Eigen::Quaterniond(R_);
    orientationTarget.w = qua_.w();
    orientationTarget.x = qua_.x();
    orientationTarget.y = qua_.y();
    orientationTarget.z = qua_.z();

    // TODO:限制总推力
    thrustTarget = accExcept_g.norm() / GRAVITATIONAL_ACC * (baseThrust); // 目标推力值
    // ROS_WARN("thrustTarget :  %.3f",thrustTarget);
    thrustTarget = min(max(thrustTarget, (double)rosParamThrustLimit[0]), (double)rosParamThrustLimit[1]);

    // 在需要输出flight_mode的地方
    ROS_WARN("flight_mode: %s", flightModeToString(flight_mode).c_str());

    // extend a static mode
    if(flight_mode == IDLE){
        thrustTarget = 0;
    }
    
    // extend docking mode
    if(flight_mode == DOCKING && init_dock){
        // // keep the ori
        // msgTargetAttitudeThrust.orientation = offb_dockini_ori.pose.orientation;
        // // msgTargetAttitudeThrust.thrust = 0.750;  //baseThrust
        // msgTargetAttitudeThrust.thrust = baseThrust;  //baseThrust
        // msgTargetAttitudeThrust.header.stamp = ros::Time::now();
        // angleTarget = quaternion2Euler_ros(offb_dockini_ori.pose.orientation);
        // ROS_INFO("[keep the ori] target Yaw = %.3f |Pitch = %.3f | Roll = %.3f | thrust = %.3f", angleTarget.z, angleTarget.y, angleTarget.x, thrustTarget);

        // add ori to roll
        msgTargetAttitudeThrust.thrust = baseThrust;  //baseThrust
        msgTargetAttitudeThrust.header.stamp = ros::Time::now();
        angleTarget = quaternion2Euler_ros(offb_dockini_ori.pose.orientation);
        angleDocking = angleTarget;
        angleDocking.x = angleTarget.x;
        orientationDocking = euler2quaternion(angleDocking.x, angleDocking.y, angleDocking.z);
        msgTargetAttitudeThrust.orientation = orientationDocking.pose.orientation;
        ROS_WARN("[keep the ori] target Yaw = %.3f |Pitch = %.3f | Roll = %.3f | thrust = %.3f", angleDocking.z, angleDocking.y, angleDocking.x, baseThrust);
    }
    // not in docking mode
    else
    {
        msgTargetAttitudeThrust.orientation = orientationTarget;
        msgTargetAttitudeThrust.thrust = thrustTarget;
        msgTargetAttitudeThrust.header.stamp = ros::Time::now();
        angleTarget = quaternion2Euler_ros(orientationTarget);
        ROS_INFO("target Yaw = %.3f |Pitch = %.3f | Roll = %.3f | thrust = %.3f", angleTarget.z, angleTarget.y, angleTarget.x, thrustTarget);
    }

    // TODO: TEST
    ROS_INFO_STREAM("CurrPos  :" << CurrPos[0] << " " << CurrPos[1] << " " << CurrPos[2]);
    ROS_INFO_STREAM("TargetPos:" << TargetPos[0] << " " << TargetPos[1] << " " << TargetPos[2]);
    ROS_INFO_STREAM("posError :" << posError[0] << " " << posError[1] << " " << posError[2]);
    ROS_INFO_STREAM("OUT pos:" << pidPX.Output << " " << pidPY.Output << " " << pidPZ.Output);
    ROS_INFO_STREAM("CurrVel  :" << CurrVel[0] << " " << CurrVel[1] << " " << CurrVel[2]);
    ROS_INFO_STREAM("TargetVel:" << TargetVel[0] << " " << TargetVel[1] << " " << TargetVel[2]);
    ROS_INFO_STREAM("velError :" << velError[0] << " " << velError[1] << " " << velError[2]);
    ROS_INFO_STREAM("OUT vel:" << pidVX.Output << " " << pidVY.Output << " " << pidVZ.Output);
    ROS_INFO_STREAM("TargetAcc:" << TargetAcc[0] << " " << TargetAcc[1] << " " << TargetAcc[2]);
    ROS_INFO_STREAM("accExcept:" << accExcept[0] << " " << accExcept[1] << " " << accExcept[2]);

    // record PID param for debug
    msgDebugPid.header.stamp = ros::Time::now();
    msgDebugPid.PError.x = posError[0];
    msgDebugPid.PError.y = posError[1];
    msgDebugPid.PError.z = posError[2];
    msgDebugPid.POut.x = pidPX.Output;
    msgDebugPid.POut.y = pidPY.Output;
    msgDebugPid.POut.z = pidPZ.Output;

    msgDebugPid.POut_P.x = pidPX.P_Out;
    msgDebugPid.POut_P.y = pidPY.P_Out;
    msgDebugPid.POut_P.z = pidPZ.P_Out;

    msgDebugPid.POut_I.x = pidPX.I_Out;
    msgDebugPid.POut_I.y = pidPY.I_Out;
    msgDebugPid.POut_I.z = pidPZ.I_Out;

    msgDebugPid.POut_D.x = pidPX.D_Out;
    msgDebugPid.POut_D.y = pidPY.D_Out;
    msgDebugPid.POut_D.z = pidPZ.D_Out;

    msgDebugPid.VError.x = velError[0];
    msgDebugPid.VError.y = velError[1];
    msgDebugPid.VError.z = velError[2];
    msgDebugPid.VOut.x = pidVX.Output;
    msgDebugPid.VOut.y = pidVY.Output;
    msgDebugPid.VOut.z = pidVZ.Output;

    msgDebugPid.VOut_P.x = pidVX.P_Out;
    msgDebugPid.VOut_P.y = pidVY.P_Out;
    msgDebugPid.VOut_P.z = pidVZ.P_Out;

    msgDebugPid.VOut_I.x = pidVX.I_Out;
    msgDebugPid.VOut_I.y = pidVY.I_Out;
    msgDebugPid.VOut_I.z = pidVZ.I_Out;

    msgDebugPid.VOut_D.x = pidVX.D_Out;
    msgDebugPid.VOut_D.y = pidVY.D_Out;
    msgDebugPid.VOut_D.z = pidVZ.D_Out;

    msgDebugPid.AOut.x = TargetAcc[0];
    msgDebugPid.AOut.y = TargetAcc[1];
    msgDebugPid.AOut.z = TargetAcc[2];
    msgDebugPid.TotalOut.x = accExcept[0];
    msgDebugPid.TotalOut.y = accExcept[1];
    msgDebugPid.TotalOut.z = accExcept[2];

    pubPID.publish(msgDebugPid);
}
// TODO
// bool landedDetection(geometry_msgs::PoseStamped CurrPos) {

//     // 检查垂直,水平运动
//     if (fabs(CurrPos.pose.position.x) > threshold_Px ||
//         fabs(CurrPos.pose.position.y) > threshold_Py ||
//         fabs(CurrPos.pose.position.z) > threshold_Pz ||) {
//         return false;
//     }

//     // 计算推力阈值
//     float min_thr = MPC_THR_MIN;
//     float hover_thr = MPPC_THR_HOVER;
//     float thr_threshold = min_thr + (hover_thr - min_thr) * 0.6;

//     // 检查推力
//     if (uav_cur_thr < thr_threshold) {
//         return true;
//     }

//     return false;
// }

/**
 * @brief main func: 将pos2pva的结果,通过调用PX4串级PID程序解算,得到需要的油门和姿态
 */
void PX4CtrlFSM::assignCurrentPositionVelocityAndYaw(const ros::Time& startTime, const offboard::PosVelAcc& droneTargetPVA) {
    relativeTime = (uav_cur_poseOdom.header.stamp - startTime).toSec();
    odomCurrPos << uav_cur_poseOdom.pose.pose.position.x, uav_cur_poseOdom.pose.pose.position.y, uav_cur_poseOdom.pose.pose.position.z;
    odomCurrVel << uav_cur_poseOdom.twist.twist.linear.x, uav_cur_poseOdom.twist.twist.linear.y, uav_cur_poseOdom.twist.twist.linear.z;
    TargetPos << droneTargetPVA.px, droneTargetPVA.py, droneTargetPVA.pz;
    TargetVel << droneTargetPVA.vx, droneTargetPVA.vy, droneTargetPVA.vz;
    TargetAcc << droneTargetPVA.ax, droneTargetPVA.ay, droneTargetPVA.az;
    TargetYaw = droneTargetPVA.yaw;

    double yaw_diff = fmod(abs(TargetYaw - yaw_last), M_PI_2);
    if (yaw_diff > yaw_thre) {
        // ROS_WARN_STREAM("The difference between expected yaw(" << TargetYaw << ")and last yaw(" << yaw_last << ")is too large");
        // ROS_WARN_STREAM("it will cause serious drift in the T265 localization data, so the previous yaw will be used");
        TargetYaw = yaw_last;
    } else {
        yaw_last = TargetYaw;
    }

    // 在这里调用PX4串级PID程序解算，并发布控制话题
    px4AttitudeCtlPVA(relativeTime, odomCurrPos, odomCurrVel, TargetPos, TargetVel, TargetAcc, TargetYaw, uav_cur_state);
    pubPx4Attitude.publish(msgTargetAttitudeThrust);
    // 可视化欧拉角姿态与油门值
    droneTargetEuler = quaternion2euler(msgTargetAttitudeThrust.orientation.x,
                                        msgTargetAttitudeThrust.orientation.y,
                                        msgTargetAttitudeThrust.orientation.z,
                                        msgTargetAttitudeThrust.orientation.w);
    msgDroneTargetEulerThrust.header.stamp = ros::Time::now();
    msgDroneTargetEulerThrust.pose.position.x = droneTargetEuler.x;
    msgDroneTargetEulerThrust.pose.position.y = droneTargetEuler.y;
    msgDroneTargetEulerThrust.pose.position.z = droneTargetEuler.z;
    msgDroneTargetEulerThrust.pose.orientation.x = msgTargetAttitudeThrust.thrust;
    pubDroneTargetEulerThrust.publish(msgDroneTargetEulerThrust);
    pubTargetPose.publish(droneTargetPVA);
}
